#include "Motors.h"
#include <boost/thread/xtime.hpp>
#include <boost/function.hpp>
#include <iomanip>

#define LEFT_MOTOR_NUMBER		0
#define LEFT_OPTO_GPIO_NUMBER	60

#define RIGHT_MOTOR_NUMBER		1
#define RIGHT_OPTO_GPIO_NUMBER	7

#define INTERVAL_MS		200

#define PID_COEF_P			10
#define PID_COEF_I			0.
#define PID_COEF_D			0.
#define PID_OUTPUT_MAX		127

#define OPTO_GREQ_DIV		10


Motors::Motors(int ResetGpioNumber,short ComNum, int BaudRate, double p, double i, double d):
SerialWrapper(ComNum, BaudRate),
m_ResetGpioNumber(ResetGpioNumber),
m_shallStop(false),
m_Thread(NULL)
{
	std::stringstream gpioNumberStr;
	gpioNumberStr << m_ResetGpioNumber;

	int ExportFd = open("/sys/class/gpio/export", O_WRONLY);
	write(ExportFd, gpioNumberStr.str().c_str(), gpioNumberStr.str().size());
	close(ExportFd);

	std::stringstream DirectionFileName;
	DirectionFileName << "/sys/class/gpio/gpio" << m_ResetGpioNumber << "/direction";

	int DirectionFd = open(DirectionFileName.str().c_str(), O_WRONLY);
	write(DirectionFd, "out", 3);
	close(DirectionFd);

	char c = '1';
	std::stringstream FileName;
	FileName << "/sys/class/gpio/gpio" << m_ResetGpioNumber << "/value";
	m_ResetGpioFd = open(FileName.str().c_str(),O_WRONLY);
	write(m_ResetGpioFd, &c, 1);
	close(m_ResetGpioFd);

	m_LeftMotor = new MotorControler(this, LEFT_MOTOR_NUMBER, p, i, d, PID_OUTPUT_MAX, LEFT_OPTO_GPIO_NUMBER, OPTO_GREQ_DIV);
	m_RightMotor = new MotorControler(this, RIGHT_MOTOR_NUMBER, p, i, d, PID_OUTPUT_MAX, RIGHT_OPTO_GPIO_NUMBER, OPTO_GREQ_DIV);
}

Motors::~Motors()
{
	this->Stop();
	delete m_LeftMotor;
	delete m_RightMotor;
	ResetActuartors();
}

void Motors::ResetActuartors()
{
	std::stringstream FileName;
	FileName << "/sys/class/gpio/gpio" << m_ResetGpioNumber << "/value";

	char c = '0';

	m_ResetGpioFd = open(FileName.str().c_str(),O_WRONLY);
	write(m_ResetGpioFd, &c, 1);
	close(m_ResetGpioFd);

	boost::this_thread::sleep(boost::posix_time::milliseconds(10));

	c = '1';
	m_ResetGpioFd = open(FileName.str().c_str(),O_WRONLY);
	write(m_ResetGpioFd, &c, 1);
	close(m_ResetGpioFd);
}

void Motors::WriteToBus(char* Message, int MessageSize)
{
	if(false == m_HandleOpened)
		return;

	std::cout << "Serial " << m_ComNum << " Write : ";
	for(int i = 0; i < MessageSize; i++)
		std::cout << std::setw(2) << std::hex << std::setfill('0') << static_cast<int>(*(Message + i)) << " ";

	std::cout << std::endl;
	write(m_Handle, Message, MessageSize);
}

void Motors::Run()
{
	m_shallStop = false;
	boost::function<void (void)>Func = boost::bind(&Motors::IterationsThread, this);
	m_Thread = new boost::thread(Func);
}

void Motors::IterationsThread()
{
	while(false == m_shallStop)
	{
		std::cout << "Iterate Motors" << std::endl;
		m_LeftMotor->Iteration();
		m_RightMotor->Iteration();
		boost::this_thread::sleep(boost::posix_time::milliseconds(INTERVAL_MS));
	}
}

void Motors::Stop()
{
	m_shallStop = true;
	if(NULL != m_Thread)
	{
		m_Thread->join();
		delete m_Thread;
	}
	std::cout << "Motors Stopped!" << std::endl;
}
